package ballFinder.behavior;

import ballFinder.environment.MotorControl;
import ballFinder.environment.SensorControl;
import lejos.robotics.subsumption.Behavior;

public class GotoBall implements Behavior {

	private boolean suppressAction;
	private SensorControl sensors;
	private MotorControl motor;
	
	public GotoBall(SensorControl sensors,MotorControl motor) {
		this.sensors = sensors;
		this.motor = motor;
	}

	@Override
	public void action() {
		motor.stop();
		suppressAction = false;
		turnToBall();
		if(!suppressAction){
			driveToBall();
		}
		motor.stop();
	}

	private void driveToBall() {
		int i=0;
		while(!suppressAction)
		{
			i++;
			if(i>3000){
				if(sensors.infraredSignalReceived()){
					i=0;
				}else{
					motor.stop_now();
					return;
				}
			}
			SensorControl.Direction direction = sensors.getInfraredDirection();
			switch(direction){
				case Left:
					return;
				case Right:
					return;
				case InFront:
				default:
					break;
			}
			motor.move(); //Muss angehalten werden.	
		}
	}

	private void turnToBall() {
		boolean left=false;
		boolean right=false;
		
		int i=0;
		
		while(!suppressAction)
		{
			i++;
			if(i>1000){
				if(sensors.infraredSignalReceived()){
					i=0;
				}else{
					motor.stop_now();
					return;
				}
			}
			SensorControl.Direction direction = sensors.getInfraredDirection();
			switch(direction){
				case Left:
					if(!left){motor.turnForever(MotorControl.TurnDirection.Left);}
					left=true;
					right=false;
					break;
				case Right:
					if(!right){motor.turnForever(MotorControl.TurnDirection.Right);}
					left=false;
					right=true;
					break;
				case InFront:
					right=false;
					left=false;
					return;
				default:
					right=false;
					left=false;
					return;
			}
		}
	}

	@Override
	public void suppress() {
		suppressAction = true;
	}

	@Override
	public boolean takeControl() {
		return sensors.infraredSignalReceived();
	}

}
